/**
 * @file bucket_transport_node.cpp
 * @brief 条筒搬运任务节点
 * @author weeksun
 * @version 1.0
 * @date 2017-09-30
 */
#include <ros/ros.h>
#include "robot_driver_base.h"
#include "bucket_transport_task.h"
#include "prosper_user_code.h"
#include "cognition_wrapper.h"


class A{
public:
  void print(const nav_msgs::OdometryConstPtr &msg){
    ROS_INFO("A.PRINT %s",msg->header.frame_id.c_str());
  }
};

// void armAjust(const RobotDriverBase::ArmAjustAck& ack){
//    std::cout<<"sate of arm :::"<<ack<<std::endl;
//    ROS_INFO("Arm Adj ack....");
// }

int main(int argc, char **argv)
{
  ros::init(argc, argv, "bucket_transport_node");
  ros::NodeHandle nh;

  //ros::Publisher marker_pub = nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
  ros::Rate r(30);
  float f = 0.0;

//   AgvAssistant::RobotDriverBasePtr robot(new AgvAssistant::RobotDriverBase(std::string("fwefwe")));
//   AgvAssistant::BucketTransport bucket(robot,std::string("bucket"));

 //   std::string driver_name_ = "adada";
 //   RobotDriverBasePtr driver_ptr_;    ///< 机器人驱动对象共享指针
 //   driver_ptr_ = RobotDriverBasePtr(new prosper_user::ProsperUser(driver_name_));
  //  driver_ptr_->registerArmAjustAckCallback(
  //        boost::bind(&armAjust,_1));
//    char order[5] = {'1','Q','X','1',0x0D};
//    driver_ptr_->armSend(order,5,1);


  // AgvAssistant::AreaManager area(2.5,0.25,1.5);


   CognitionWrapper wrapper;

   r.sleep();
  ros::spin();

//   while (ros::ok())
//   {



//   std::vector<tf::Vector3> points;
//   tf::Vector3 a;
//   a.setX(-2*2.2);
//   a.setY(-1*2.2);
//   a.setZ(0);
//   points.push_back(a);
//   a.setX(2*2.2);
//   a.setY(3*2.2);
//   a.setZ(0);
//   points.push_back(a);
//   a.setX(0);
//   a.setY(5*2.2);
//   a.setZ(0);
//   points.push_back(a);
//   a.setX(-4*2.2);
//   a.setY(1*2.2);
//   a.setZ(0);
//   points.push_back(a);

//   AgvAssistant::AreaManager::AreaGrid area_visual_;

//   tf::Vector3 dock;
//   dock.setX(1);
//   dock.setY(2);
//   dock.setZ(0);

//   if(area.generateArea(0,AgvAssistant::AreaManager::AREA_PUT,points,dock))
//   {
//     area.areaVisualization(area_visual_);
//   }

//   marker_pub.publish(area_visual_.points);
//   marker_pub.publish(area_visual_.line_strip);
//   marker_pub.publish(area_visual_.line_list);


//   tf::Transform pose;
//   tf::Vector3 pose_origen;
//   pose_origen.setX(1);
//   pose_origen.setY(2.4);
//   pose_origen.setZ(0);
//   pose.setOrigin(pose_origen);
//   tf::Quaternion pose_rotate;
//   pose_rotate.setRotation(tf::Vector3(0,0,1),1*3.1415926);
//   pose.setRotation(pose_rotate);

//   AgvAssistant::AreaManager::HotArea hotarea;
//   hotarea.hot_point.setX(1);
//   hotarea.hot_point.setY(2.0);
//   hotarea.hot_point.setZ(0);

//  // bool aa =area.isRobotInPosition(hotarea,pose);

//   bool aa = area.isRobotInDock(pose);
//   std::cout<<"aaa::"<<aa<<std::endl;
//   std::cout<<"x;:"<<pose.getOrigin().getX()<<std::endl;
//   std::cout<<"y;:"<<pose.getOrigin().getY()<<std::endl;
//   std::cout<<"z;:"<<pose.getOrigin().getZ()<<std::endl;
//   r.sleep();

//   f += 0.04;
//   }

//   robot->test();

//   ros::spin();

}
